#include <stdio.h>
#include "pid.h"


PID_TypeDef  g_speed_pid;                       /* 速度PID参数结构体 */
pid_inter_t g_pid_inter;


volatile float g_d_value_max_hold = -999.0f;
volatile float g_d_value_min_hold = 999.0f;
volatile float adaptive_ki = S_KI;

/**
 * @brief       pid初始化
 * @param       无
 * @retval      无
 */
void pid_init(void)
{
    g_speed_pid.target_value = 0;       /* 设定目标值 */
    g_speed_pid.actual_value = 0.0;  /* 期望值输出 */
    g_speed_pid.sum_error = 0.0;     /* 积分值 */
    g_speed_pid.error = 0.0;        /* Error[1] */
    g_speed_pid.last_error = 0.0;    /* Error[-1] */
    g_speed_pid.prev_error = 0.0;    /* Error[-2] */
    g_speed_pid.proportion = S_KP;  /* 比例常数 Proportional Const */
    g_speed_pid.integral = S_KI;    /* 积分常数 Integral Const */
    g_speed_pid.derivative = S_KD;  /* 微分常数 Derivative Const */ 
    g_speed_pid.ing_max = PID_I_MAX;
    g_speed_pid.ing_min = 0;
    g_speed_pid.out_max = PID_MAX_OUTPUT;       /* 输出限制 */
    g_speed_pid.out_min = 0;    
}


/**
 * @brief       闭环PID控制算法设计
 * @note        通过宏 INCR_LOCT_SELECT 选择使用位置式算法/增量式算法
 * @param       *PID：PID结构体句柄所对应的目标值
 * @param       Feedback_value ： 实际值
 * @retval      目标控制量
 */
int32_t pid_ctrl(PID_TypeDef *PID,float Feedback_value)
{
    PID->error = (float)(PID->target_value - Feedback_value);                   /* 速度档位偏差*/

    if(Feedback_value <= 400){
        adaptive_ki = (Feedback_value/400);
        if(adaptive_ki < 0.3f){
            adaptive_ki = 0.3f;
        }
    } else {
        adaptive_ki = 1.0f;
    }
    PID->sum_error += (PID->error*adaptive_ki);


    g_pid_inter.p_value = PID->proportion * PID->error;
    g_pid_inter.i_value = PID->integral * PID->sum_error;
    g_pid_inter.d_value = PID->derivative * (PID->error - PID->last_error);
    g_pid_inter.sum_value = g_pid_inter.p_value + g_pid_inter.i_value + g_pid_inter.d_value;//用于调试
    PID->actual_value = g_pid_inter.sum_value;

    PID->last_error = PID->error;

    // 积分抗饱和处理
    if(PID->sum_error > g_speed_pid.ing_max)
    {
        PID->sum_error = g_speed_pid.ing_max;
    }
    else if(PID->sum_error < g_speed_pid.ing_min)
    {
        PID->sum_error = g_speed_pid.ing_min;
    }

    //限制输出饱和
    if(PID->actual_value > PID->out_max)
    {
        PID->actual_value = PID->out_max;
    }
    else if(PID->actual_value <PID->out_min)
    {
        PID->actual_value = PID->out_min;
    }

    // 用来记录D项的最大值和最小值，用于调试，否则d值常常在上位机看不到
    if (g_pid_inter.d_value > g_d_value_max_hold) {
        g_d_value_max_hold = g_pid_inter.d_value;
    }
    if (g_pid_inter.d_value < g_d_value_min_hold) {
        g_d_value_min_hold = g_pid_inter.d_value;
    }

    return ((int32_t)(PID->actual_value));                                   /* 返回实际控制数值*/
}


void pid_set_target_value(int32_t target_value)
{
    g_speed_pid.target_value = target_value;
    // printf("s: %d\r\n", target_value);
}

/**
 * @brief       重置PID积分项（用于安全换向）
 * @param       无
 * @retval      无
 */
void pid_reset_integral(void)
{
    g_speed_pid.sum_error = 0.0f;
    // printf("PID integral term reset for safe direction change\r\n");
}

//这个数据是上传到上位机便于观察，开始时清零
void pid_inter_data_reset(void)
{
    g_pid_inter.p_value = 0;
    g_pid_inter.i_value = 0;
    g_pid_inter.d_value = 0;
    g_pid_inter.sum_value = 0;
}


// int32_t pid_ctrl(PID_TypeDef *PID, float Feedback_value)
// {
//     // 1. 计算当前误差
//     PID->error = (float)(PID->target_value - Feedback_value);

//     // 2. 计算P, I, D各项的值
//     // 注意：这里的 i_value 是基于上一次的 sum_error 计算的，或者基于本次累加后的 sum_error
//     // 两种实现方式都可以，这里按您原来的逻辑，先累加
//     PID->sum_error += PID->error;

//     g_pid_inter.p_value = PID->proportion * PID->error;
//     g_pid_inter.i_value = PID->integral * PID->sum_error;
//     g_pid_inter.d_value = PID->derivative * (PID->error - PID->last_error);

//     // 3. 计算未限制的总输出
//     PID->actual_value = g_pid_inter.p_value + g_pid_inter.i_value + g_pid_inter.d_value;
    
//     // 4. 更新上次误差
//     PID->last_error = PID->error;

//     // 5. 输出限幅与积分抗饱和处理
//     if(PID->actual_value > PID->out_max)
//     {
//         // 如果输出超过上限，则将输出限制在上限
//         PID->actual_value = PID->out_max;

//         // 【积分抗饱和】如果当前误差是正的（意味着想让输出更大），
//         // 则撤销本次积分累加，防止积分持续增大。
//         if (PID->error > 0)
//         {
//             PID->sum_error -= PID->error;
//         }
//     }
//     else if(PID->actual_value < PID->out_min) // 建议使用 out_min 而不是固定的 0
//     {
//         // 如果输出低于下限，则将输出限制在下限
//         PID->actual_value = PID->out_min;

//         // 【积分抗饱和】如果当前误差是负的（意味着想让输出更小），
//         // 则撤销本次积分累加，防止积分持续减小。
//         if (PID->error < 0)
//         {
//             PID->sum_error -= PID->error;
//         }
//     }

//     // 6. 返回最终被限制的、安全的控制值
//     return ((int32_t)(PID->actual_value));
// }

